#include <assert.h>
#include "csbmath.h"
#include "utils.h"

#ifdef __cplusplus
extern "C" {
#endif

/*********************| 内部宏 |*********************/
#define LOOKUP_BITS		8							
#define EXP_POS			23							
#define EXP_BIAS		127							
#define LOOKUP_POS		(EXP_POS-LOOKUP_BITS)
#define SEED_POS		(EXP_POS-8)
#define SQRT_TABLE_SIZE		(2<<LOOKUP_BITS)
#define LOOKUP_MASK		(SQRT_TABLE_SIZE-1)
/*********************| 内部变量 |*********************/
static bool		_inited = false;
union _flint {
	int32_t		i;	
	float		f;
};
static int32_t		_iSqrt[SQRT_TABLE_SIZE];
/*********************| 外部接口 |*********************/
CSB_DLL void CSB_math_init()
{
	CSB_YES_VRET(_inited);
	// 初始化一个用来查询开方的表
	union _flint fi, fo;
	int i = 0;
	for (; i < SQRT_TABLE_SIZE; ++i ) {
		fi.i = ((EXP_BIAS-1) << EXP_POS) | (i << LOOKUP_POS);
		fo.f = (float)( 1.0 / sqrtf(fi.f) );
		_iSqrt[i] = ((int32_t)(((fo.i + (1<<(SEED_POS-2))) >> SEED_POS) & 0xFF))<<SEED_POS;
	}
	_iSqrt[SQRT_TABLE_SIZE / 2] = ((int32_t)(0xFF))<<(SEED_POS);
	_inited = true;
}

CSB_DLL float CSB_sqrt(float x)
{
	assert(_inited);
	int32_t a = ((union _flint*)(&x))->i;
	union _flint seed;
	double y = x * 0.5f;
	seed.i = (( ( (3*EXP_BIAS-1) - ( (a >> EXP_POS) & 0xFF) ) >> 1)<<EXP_POS) | _iSqrt[(a >> (EXP_POS-LOOKUP_BITS)) & LOOKUP_MASK];
	double r = seed.f;
	r = r * ( 1.5f - r * r * y );
	r = r * ( 1.5f - r * r * y );
	return (float)r * x;
}

CSB_DLL int CSB_abs(int x)
{
   int y = x >> 31;
   return ( ( x ^ y ) - y );
}

CSB_DLL float CSB_fabs(float x)
{
	int tmp = *(int *)( &x );
	tmp &= 0x7FFFFFFF;
	return *(float *)( &tmp );
}

CSB_DLL float CSB_angle_normalize_360(float x)
{
	if ( ( x >= 360.0f ) || ( x < 0.0f ) ) {
		x -= floor( x / 360.0f ) * 360.0f;
	}
	return x;
}

CSB_DLL float CSB_angle_normalize_180(float x)
{
	x = CSB_angle_normalize_360(x);
	if ( x > 180.0f ) {
		x -= 360.0f;
	}
	return x;
}

CSB_DLL void CSB_rotate(float* x, float* y, float degree)
{
	//1.取负，获得360°内的数学库角度。计算特殊角度
	degree = CSB_fmod(-degree, 360.0f);
	if (!(degree)) return;
	const float tx = *x;
	const float ty = *y;

	float tmp = CSB_fmod(degree , 180);
	if (ERROR_IS_TRUE(tmp, 0.01f)) {
		*x = -tx;
		*y = -ty;
		return;
	}
	tmp = CSB_fmod(degree , 90.0f);
	if (ERROR_IS_TRUE(tmp,0.01f)) {
		if (CSB_fabs(degree) > 90.0f) {
			// 270 绝对值缩放到90以获得象限的趋势
			degree = (degree < 0.0f) ? (360.0f + degree) : (360.0f - degree);
		}
		if (degree < 0) {
			// 象限加1运算
			*y = -*x;
			*x = ty;
		} else {
			// 象限减一运算
			*x = -*y;
			*y = tx;
		}
		return;
	}
	degree = CSB_DEG2RAD(degree);
	const float tcos = CSB_cos(degree);
	const float tsin = CSB_sin(degree);
	*x = tx * tcos - ty * tsin;
	*y = tx * tsin + ty * tcos;
}


CSB_DLL float CSB_rotate_p2p(const pos_t* ori, const pos_t* target)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_RET(ori && target, CSBERR_PARAM, 0);
	float a = (target->y - ori->y);
	float b = (target->x - ori->x);
	CSB_YES_RET(0 == a && 0 == b, 0);
	float t = 90.0f;
	if (0.0f != b) {
		t =  CSB_RAD2DEG(CSB_atan(a/b));
	}
	if (t < 0) {
		if (a > 0)
			t += 180.0f;
	} else if (t > 0) {
		if (a < 0)
			t += 180.0f;
	} else if (a < 0) {
		t += 180.0f;
	}
	return t;
}

#ifdef __cplusplus
} // end of extern C
#endif

